
#include "Line.h"
#include "bits/stdc++.h"
#include "demo1.h"

using namespace std;
serial::Serial ser;
vector<int>    q1 = {
    //    STRIGHT,
    //    RIGHT,
    //    STRIGHT,
    //    LEFT,
    //    LEFT,
    //    DAOCHE

    //    STRIGHT,
    //    STRIGHT,
    //    LEFT,
    //    STRIGHT,
    //    DAYOUZHUAN,
    //    DAOCHE

//    STRIGHT,
//    RIGHT,
//    STRIGHT,
//    LEFT,
//    LEFT,
//    DAOCHEYOUZHAUAN,
//    DAOCHE
//
        STRIGHT,
        STRIGHT,
        RIGHT,
        LEFT,
        DAOCHE};


auto tl = new TraceLine(q1);

int times = 0;

void USART_SEND(serial::Serial& ser1, int angle, int speed);

void da_you_zhuan(const sensor_msgs::ImageConstPtr& msg)
{
    Mat img_resize;
    Mat img1 = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
    resize(img1, img_resize, Size(640, 480));
    char gear      = '0';
    Mat  img_blue  = img_resize(Range(360, 480), Range(0, 640));
    Line temp_blue = tl->calculate_blue(img_blue);
    gear           = tl->strategy_blue(temp_blue);
    if ( gear == '0' ) {
        cout << "\033[31m\033[44mbegin\033[0m" << endl;
        int k = 1;
        // 直行
        for ( int i = 0; i < 0.3 * 100 * k; i++ ) {
            USART_SEND(ser, 0, 4);
            usleep(10000);
        }
        // 右转
        cout << "\033[31m\033[44mright\033[0m" << endl;
        for ( int i = 0; i < 14 * 100 * k; i++ ) {
            USART_SEND(ser, 200, 4);
            usleep(10000);
        }
        // 结束
        cout << "\033[31m\033[44mend\033[0m" << endl;

        tl->q.pop();
        ros::param::set("state", tl->q.front());
    }
    else {
        USART_SEND(ser, (int)(gear - '3') * 100, 4);
    }
}

void imageCallback(const sensor_msgs::ImageConstPtr& msg)
{
    // 更新light
    int light = RED_LIGHT;
    ros::param::get("light", light);
    if ( light == RED_LIGHT ) {
        USART_SEND(ser, 0, 0);
        return;
    }
    ros::param::get("state", tl->state);
    ros::param::get("stim", tl->stim);

    if ( tl->state == END ) {
        USART_SEND(ser, 0, 0);
        return;
    }
    else if ( tl->state == DAYOUZHUAN ) {
        da_you_zhuan(msg);
    }
    else if ( tl->state == BIZHANG ) {
        int gear_t;
        ros::param::get("gear", gear_t);
        USART_SEND(ser, (gear_t - 3) * 100, 4);
    }
    else if ( tl->state == DAOCHE ) {
        int gear_t, speed_t;
        ros::param::get("gear", gear_t);
        ros::param::get("speed", speed_t);
        USART_SEND(ser, gear_t, speed_t);
        cout << "zhixing zheli" << endl;
    }
    else if ( tl->state == STRIGHT || tl->state == LEFT || tl->state == RIGHT || tl->state==DAOCHEYOUZHAUAN ) {
        Mat  img    = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8)->image;
        char gear_t = tl->line_loop(img, ser);

        USART_SEND(ser, (int)(gear_t - '3') * 100, 4);
    }
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "mix");
    ros::NodeHandle nh;
    ros::Subscriber img_rcv =
        nh.subscribe("/usb_cam/image_raw", 1, &imageCallback);
    // 鎵撳紑涓插彛
    int port;
    ros::param::get("ttyUSB", port);
    port == 0 ? ser.setPort("/dev/ttyUSB0") : ser.setPort("/dev/ttyUSB1");
    ser.setBaudrate(115200);
    serial::Timeout to = serial::Timeout::simpleTimeout(3000);
    ser.setTimeout(to);
    ser.open();

    if ( ser.isOpen() ) {
        ROS_INFO_STREAM("Succeed to open port");
    }
    ros::Rate looprate(50);
#if IMG_WRITE
    tl->f.open("/home/nano/Desktop/src/src.txt", ios::in | ios::out);
#endif
    while ( ros::ok() ) {
        ros::spinOnce();
        looprate.sleep();
    }
#if IMG_WRITE
    tl->f.close();
#endif
    return 0;
}

void USART_SEND(serial::Serial& ser1, int angle, int speed)
{
    auto* tx_buf = new uint8_t[9];
    // 左右转标记，angle > 0 右转
    if ( angle > 0 ) {
        tx_buf[0] = 'R';
    }
    else {
        tx_buf[0] = 'L';
    }

    // 角度大小，最大幅度20°
    angle = abs(angle);
    if ( angle > 200 ) {
        tx_buf[1] = '2';
        tx_buf[2] = '0';
        tx_buf[3] = '0';
    }
    else {
        for ( int i = 1; i < 4; i++ ) {
            tx_buf[4 - i] = (angle % 10) + '0';
            angle /= 10;
        }
    }

    // 速度方向，前进为W，后退为S
    if ( speed > 0 ) {
        tx_buf[4] = 'W';
    }
    else {
        tx_buf[4] = 'S';
    }

    // 速度大小，最大为5，最小为0，即停车
    speed     = abs(speed);
    tx_buf[5] = speed + '0';

    // 结尾处理
    tx_buf[6] = '\r';
    tx_buf[7] = '\n';
    tx_buf[8] = '\0';

    // 发送处理
    ser.write(tx_buf, 8);
    times++;
    if ( times == 9 ) {
        cout << "\033[31mstate is : " << tl->state << "\033[0m" << endl;
        cout << "\033[31m\033[1mtx_buf : " << tx_buf << "\033[0m" << endl;
        times = 0;
    }

    delete[] tx_buf;
}
